
#include "gwGpio.h"
#include "xil_printf.h"

XGpio Gpio0; /* The Instance of the GPIO 0 Driver */
XGpio Gpio1; /* The Instance of the GPIO 1 Driver */

u32 dir_mask0 = 0x31A49000; /* direction out = 0, direction in = 1 */
u32 dir_mask1 = 0x001C0349;

int init_gpio(){
    int Status;

    /* Initialize the GPIO driver */
    Status = XGpio_Initialize(&Gpio0, GPIO_0_DEVICE_ID);
    if (Status != XST_SUCCESS) {
        xil_printf("Gpio0 Initialization Failed\r\n");
        return XST_FAILURE;
    }

    Status = XGpio_Initialize(&Gpio1, GPIO_1_DEVICE_ID);
    if (Status != XST_SUCCESS) {
        xil_printf("Gpio1 Initialization Failed\r\n");
        return XST_FAILURE;
    }

    /* Set the direction for all signals as inputs except the EN_ADC output */

    XGpio_SetDataDirection(&Gpio0, dir_mask0);
    XGpio_SetDataDirection(&Gpio1, dir_mask1);

    return XST_SUCCESS;
}

#define CONTROL_7044_ADDRESS 0x44a50000
// active high
void reset7044(){
    u32 value = gw_ReadReg(CONTROL_7044_ADDRESS);
    u32 mask = 0x00000080;
    value = value | 0x00000080;
    gw_sleep(DELAY_WR);
    gw_WriteReg(CONTROL_7044_ADDRESS, value);
}

void set7044(){
    u32 value = gw_ReadReg(CONTROL_7044_ADDRESS);
    u32 mask = 0xffffff7f;
    value = value & 0xffffff7f;
    gw_sleep(DELAY_WR);
    gw_WriteReg(CONTROL_7044_ADDRESS, value);
} 

void gpio_startup(){
    reset_gpio(O_LED1_T);
    set_gpio(O_LED1_T);
    reset_gpio(O_LED1_T);

    reset_gpio(O_LED2_T);
    set_gpio(O_LED2_T);
    reset_gpio(O_LED2_T);

    reset7044();
    set7044();

    reset_gpio(O_LMX_ADC_RSTN);
    set_gpio(O_LMX_ADC_RSTN);

    reset_gpio(O_LMX_DAC_RSTN);
    set_gpio(O_LMX_DAC_RSTN);

    reset_gpio(O_DAC_RSTN);
    gw_sleep(DELAY_WR);
    set_gpio(O_DAC_RSTN);

    set_gpio(O_SOFT_RST);
    gw_sleep(DELAY_WR);
    reset_gpio(O_SOFT_RST);

    reset_gpio(O_SYNC_TO_PL);

    set_gpio(O_FAN_EN);
}

int gpio_pos(u32 gpio_io){
    u32 tmp_io = gpio_io;
    int cnt = 0;
    while(tmp_io != 0){
        cnt = cnt + 1;
        tmp_io = tmp_io >> 1;
    }
    return cnt;
}

/* direction out = 0, direction in = 1 */
u32 change_direction_out(u32 gpio_io){
    u32 dir_mask_pre = 0x0;
    u32 dir_mask = 0x0;

    XGpio *Gpio = NULL; 
    if(gpio_io > FLAG_GPIO_1){
        Gpio = &Gpio1;
        gpio_io = gpio_io - FLAG_GPIO_1;
    }else{
        Gpio = &Gpio0;
    }

    dir_mask_pre = XGpio_GetDataDirection(Gpio);

    dir_mask = dir_mask_pre & (~gpio_io);
    XGpio_SetDataDirection(Gpio, dir_mask);
    return dir_mask_pre;  
}

u32 change_direction_in(u32 gpio_io){
    u32 dir_mask_pre = 0x0;
    u32 dir_mask = 0x0;

    XGpio *Gpio = NULL; 
    if(gpio_io > FLAG_GPIO_1){
        Gpio = &Gpio1;
        gpio_io = gpio_io - FLAG_GPIO_1;
    }else{
        Gpio = &Gpio0;
    }

    dir_mask_pre = XGpio_GetDataDirection(Gpio);
    dir_mask = dir_mask_pre | gpio_io;
    XGpio_SetDataDirection(Gpio, dir_mask);
    return dir_mask_pre;  
}

int read_gpio(u32 gpio_io){
    u32 Data;
    change_direction_in(gpio_io);
    XGpio *Gpio = NULL;
    if(gpio_io > FLAG_GPIO_1){
        Gpio = &Gpio1;
        gpio_io = gpio_io - FLAG_GPIO_1;
    }else{
        Gpio = &Gpio0;
    }
    Data = XGpio_DiscreteRead(Gpio);
    // xil_printf("read_gpio : = 0x%x\r\n", Data);
    int gpio_num = gpio_pos(gpio_io);
    int val = ( Data >> (gpio_num - 1) ) & 0x1;
    return val;
}

void set_gpio(u32 gpio_io){
    change_direction_out(gpio_io);
    XGpio *Gpio = NULL; 
    if(gpio_io > FLAG_GPIO_1){
        Gpio = &Gpio1;
        gpio_io = gpio_io - FLAG_GPIO_1;
    }else{
        Gpio = &Gpio0;
    }
    XGpio_DiscreteSet(Gpio, gpio_io);
}

void reset_gpio(u32 gpio_io){
    change_direction_out(gpio_io);
    XGpio *Gpio = NULL; 
    if(gpio_io > FLAG_GPIO_1){
        Gpio = &Gpio1;
        gpio_io = gpio_io - FLAG_GPIO_1;
    }else{
        Gpio = &Gpio0;
    }
    XGpio_DiscreteClear(Gpio, gpio_io);
}


void gpio_loopback(){
    set_gpio(O_MODE_SEL);
    gw_sleep(DELAY_WR);
    set_gpio(O_MODE_SEL1);
    gw_sleep(DELAY_WR);
    set_gpio(O_MODE_SEL2);
}

void gpio_normal(){
    reset_gpio(O_MODE_SEL);
    gw_sleep(DELAY_WR);
    set_gpio(O_MODE_SEL1);
    gw_sleep(DELAY_WR);
    set_gpio(O_MODE_SEL2);
}

void gpio_rx_out(){
    reset_gpio(O_MODE_SEL);
    gw_sleep(DELAY_WR);
    reset_gpio(O_MODE_SEL1);
}

void gpio_tx_out(){
    reset_gpio(O_MODE_SEL2);
}